﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using OpenTK;
using OpenTK.Graphics;

namespace LasRendering
{
    public static class LasHelper
    {
        private static string lazFilename;

        public static QuadTree CreateQuadTree(string filename)
        {
            var minPosition = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
            var maxPosition = new Vector3(float.MinValue, float.MinValue, float.MinValue);
            float vertexAvgSize;

            using (FileStream fs = new FileStream(filename, FileMode.Open, FileAccess.Read, FileShare.Read))
            {
                using (BinaryReader br = new BinaryReader(fs))
                {
                    var header = ReadStructure<LasHeader_v1_3>(br);
                    Debug.Assert(new string(header.FileSignature) == "LASF");
                    Debug.Assert(header.VersionMajor == 1);
                    Debug.Assert(header.NumberOfPointRecords <= int.MaxValue);

                    int numVertex = (int)header.NumberOfPointRecords;

                    if (header.PointDataFormatID == 6 || header.PointDataFormatID == 7)
                    {
                        vertexAvgSize = (float)header.ScaleFactorX;
                    }
                    else
                    {
                        vertexAvgSize = ComputeVertexAvgSize(header.MinX, header.MaxX, header.MinY, header.MaxY, numVertex);
                    }

                    QuadTree quadTree = new QuadTree(
                        new Vector2((float)header.MinX, (float)header.MinY),
                        new Vector2((float)header.MaxX, (float)header.MaxY),
                        (int)header.NumberOfPointRecords);
                    quadTree.Count = numVertex;
                    quadTree.VertexAvgSize = vertexAvgSize;

                    fs.Seek(header.OffsetToPointData, SeekOrigin.Begin);

                    for (int i = 0; i < numVertex; i++)
                    {
                        Vertex vertex = ReadSingleVertex(br, ref header);

                        quadTree.AddVertexIndex(vertex.Position.Xy, i);

                        minPosition.X = Math.Min(minPosition.X, vertex.Position.X);
                        minPosition.Y = Math.Min(minPosition.Y, vertex.Position.Y);
                        minPosition.Z = Math.Min(minPosition.Z, vertex.Position.Z);
                        maxPosition.X = Math.Max(maxPosition.X, vertex.Position.X);
                        maxPosition.Y = Math.Max(maxPosition.Y, vertex.Position.Y);
                        maxPosition.Z = Math.Max(maxPosition.Z, vertex.Position.Z);
                    }

                    quadTree.MinPosition = minPosition;
                    quadTree.MaxPosition = maxPosition;
                    quadTree.VertexAvgSize = ComputeVertexAvgSize(maxPosition.X, minPosition.X, maxPosition.Y, minPosition.Y, numVertex);

                    return quadTree;
                }
            }
        }

        private static float ComputeVertexAvgSize(double minX, double maxX, double minY, double maxY, int numPoints)
        {
            return (float)Math.Sqrt((maxX - minX) * (maxY - minY) / numPoints * Math.Sqrt(2));
        }

        public static List<Vertex> ReadVertices(string filename, QuadTreeVertexIndexLink vertexIndexLink, int expectedNumOfVertices)
        {
            if (expectedNumOfVertices == 0)
            {
                return new List<Vertex>();
            }

            int gap = vertexIndexLink.Count / expectedNumOfVertices;
            if (gap < 1)
            {
                gap = 1;
            }

            bool isLazFile = false;
            FileStream classificationFileStream = null;
            if (Path.GetExtension(filename).ToLower() == ".las")
            {
                var classificationFilename = Path.ChangeExtension(filename, ".bin");
                if (File.Exists(classificationFilename))
                {
                    classificationFileStream = new FileStream(classificationFilename, FileMode.Open, FileAccess.Read, FileShare.Read);
                }
            }
            else
            {
                isLazFile = true;
            }

            var vertexList = new List<Vertex>(Math.Min(vertexIndexLink.Count, expectedNumOfVertices));
            int index = 0;

            if (isLazFile)
            {
                lazLock.EnterReadLock();
            }
            using (FileStream fs = new FileStream(filename, FileMode.Open, FileAccess.Read, FileShare.Read))
            {
                using (BinaryReader br = new BinaryReader(fs))
                {
                    var header = ReadStructure<LasHeader_v1_3>(br);
                    
                    foreach (var indexLinkNode in vertexIndexLink)
                    {
                        fs.Seek(header.OffsetToPointData + (long)indexLinkNode.StartIndex * header.PointDataRecordLength, SeekOrigin.Begin);
                        if (classificationFileStream != null)
                        {
                            classificationFileStream.Seek(indexLinkNode.StartIndex, SeekOrigin.Begin);
                        }

                        Vertex startWallVertex = null;
                        for (int i = indexLinkNode.StartIndex; i <= indexLinkNode.EndIndex; i++)
                        {
                            if (index % gap == 0 || startWallVertex != null)
                            {
                                Vertex vertex = ReadSingleVertex(br, ref header);
                                vertex.Index = i;

                                if (classificationFileStream != null)
                                {
                                    vertex.Classification = GetVertexClassification((LasPointClassification)classificationFileStream.ReadByte());
                                }

                                if (header.PointDataFormatID != 6 && header.PointDataFormatID != 7)
                                {
                                    // Actual vertex normal will be calculated in VertexHelper
                                    vertex.Normal = new Vector3(0, 0, 1);
                                }
                                else if (vertex.Classification == VertexClassification.Wall)
                                {
                                    if (vertex.WallNormal.X == -1)
                                    {
                                        startWallVertex = vertex;
                                    }
                                    else if (vertex.WallNormal.X == 1)
                                    {
                                        if (startWallVertex != null)
                                        {
                                            vertexList.Add(startWallVertex);
                                            vertexList.Add(vertex);
                                            startWallVertex = null;
                                        }
                                    }

                                    goto NextVertex;
                                }

                                vertexList.Add(vertex);
                            }
                            else
                            {
                                fs.Seek(header.PointDataRecordLength, SeekOrigin.Current);
                                if (classificationFileStream != null)
                                {
                                    classificationFileStream.Seek(1, SeekOrigin.Current);
                                }
                            }

                        NextVertex:
                            index++;
                        }
                    }
                }
            }
            if (isLazFile)
            {
                lazLock.ExitReadLock();
            }

            if (classificationFileStream != null)
            {
                classificationFileStream.Close();
            }

            return vertexList;
        }

        private static Vertex ReadSingleVertex(BinaryReader br, ref LasHeader_v1_3 header)
        {
            Vertex vertex = new Vertex();
            switch (header.PointDataFormatID)
            {
                case 0:
                    var point0 = ReadStructure<LasPointDataRecordFormat0_v1_3>(br);
                    vertex.Position.X = (float)((point0.X * header.ScaleFactorX) + header.OffsetX);
                    vertex.Position.Y = (float)((point0.Y * header.ScaleFactorY) + header.OffsetY);
                    vertex.Position.Z = (float)((point0.Z * header.ScaleFactorZ) + header.OffsetZ);
                    vertex.Classification = GetVertexClassification((LasPointClassification)point0.Classification);
                    break;

                case 1:
                    var point1 = ReadStructure<LasPointDataRecordFormat1_v1_3>(br);
                    vertex.Position.X = (float)((point1.X * header.ScaleFactorX) + header.OffsetX);
                    vertex.Position.Y = (float)((point1.Y * header.ScaleFactorY) + header.OffsetY);
                    vertex.Position.Z = (float)((point1.Z * header.ScaleFactorZ) + header.OffsetZ);
                    vertex.Classification = GetVertexClassification((LasPointClassification)point1.Classification);
                    break;

                case 2:
                    var point2 = ReadStructure<LasPointDataRecordFormat2_v1_3>(br);
                    vertex.Position.X = (float)((point2.X * header.ScaleFactorX) + header.OffsetX);
                    vertex.Position.Y = (float)((point2.Y * header.ScaleFactorY) + header.OffsetY);
                    vertex.Position.Z = (float)((point2.Z * header.ScaleFactorZ) + header.OffsetZ);
                    vertex.Classification = GetVertexClassification((LasPointClassification)point2.Classification);
                    vertex.Color = new Color4(
                        (float)point2.Red / ushort.MaxValue,
                        (float)point2.Green / ushort.MaxValue,
                        (float)point2.Blue / ushort.MaxValue,
                        1);
                    break;

                case 3:
                    var point3 = ReadStructure<LasPointDataRecordFormat3_v1_3>(br);
                    vertex.Position.X = (float)((point3.X * header.ScaleFactorX) + header.OffsetX);
                    vertex.Position.Y = (float)((point3.Y * header.ScaleFactorY) + header.OffsetY);
                    vertex.Position.Z = (float)((point3.Z * header.ScaleFactorZ) + header.OffsetZ);
                    vertex.Classification = GetVertexClassification((LasPointClassification)point3.Classification);
                    vertex.Color = new Color4(
                        (float)point3.Red / ushort.MaxValue,
                        (float)point3.Green / ushort.MaxValue,
                        (float)point3.Blue / ushort.MaxValue,
                        1);
                    break;

                case 4:
                    var point4 = ReadStructure<LasPointDataRecordFormat4_v1_3>(br);
                    vertex.Position.X = (float)((point4.X * header.ScaleFactorX) + header.OffsetX);
                    vertex.Position.Y = (float)((point4.Y * header.ScaleFactorY) + header.OffsetY);
                    vertex.Position.Z = (float)((point4.Z * header.ScaleFactorZ) + header.OffsetZ);
                    vertex.Classification = GetVertexClassification((LasPointClassification)point4.Classification);
                    break;

                case 5:
                    var point5 = ReadStructure<LasPointDataRecordFormat5_v1_3>(br);
                    vertex.Position.X = (float)((point5.X * header.ScaleFactorX) + header.OffsetX);
                    vertex.Position.Y = (float)((point5.Y * header.ScaleFactorY) + header.OffsetY);
                    vertex.Position.Z = (float)((point5.Z * header.ScaleFactorZ) + header.OffsetZ);
                    vertex.Normal.X = point5.Xt;
                    vertex.Normal.Y = point5.Yt;
                    vertex.Normal.Z = point5.Zt;
                    vertex.Classification = GetVertexClassification((LasPointClassification)point5.Classification);
                    vertex.Color = new Color4(
                        (float)point5.Red / ushort.MaxValue,
                        (float)point5.Green / ushort.MaxValue,
                        (float)point5.Blue / ushort.MaxValue,
                        1);
                    break;

                case 6:
                    var point6 = ReadStructure<LasPointDataRecordFormat6_Custom>(br);
                    vertex.Position = point6.Position;
                    vertex.Classification = GetVertexClassification(point6.Classification);
                    vertex.Color = point6.Color;
                    vertex.Normal = point6.Normal;
                    vertex.WallNormal = point6.WallNormal;
                    vertex.GroundLevel = point6.GroundLevel;
                    break;

                case 7:
                    var point7 = ReadStructure<LasPointDataRecordFormat6_Custom>(br);
                    vertex.Position = point7.Position;
                    vertex.Classification = GetVertexClassification(point7.Classification);
                    float weight = point7.Color.A * byte.MaxValue;
                    if (weight == 0)
                    {
                        vertex.Color = new Color4(0, 0, 0, 0);
                    }
                    else
                    {
                        vertex.Color = new Color4(point7.Color.R / weight, point7.Color.G / weight, point7.Color.B / weight, 1);
                    }
                    vertex.Normal = point7.Normal;
                    vertex.WallNormal = point7.WallNormal;
                    vertex.GroundLevel = point7.GroundLevel;
                    break;

                default:
                    Debug.Fail(null);
                    break;
            }
            return vertex;
        }

        private static T ReadStructure<T>(BinaryReader br) where T : struct
        {
            byte[] buf = br.ReadBytes(Marshal.SizeOf(typeof(T)));
            GCHandle handle = GCHandle.Alloc(buf, GCHandleType.Pinned);
            T val = (T)Marshal.PtrToStructure(handle.AddrOfPinnedObject(), typeof(T));
            handle.Free();
            return val;
        }

        private static void WriteStructure<T>(BinaryWriter bw, T value) where T : struct
        {
            int size = Marshal.SizeOf(typeof(T));
            IntPtr ptr = Marshal.AllocHGlobal(size);
            byte[] buf = new byte[size];
            Marshal.StructureToPtr(value, ptr, false);
            Marshal.Copy(ptr, buf, 0, size);
            Marshal.FreeHGlobal(ptr);
            bw.Write(buf);
        }

        private static VertexClassification GetVertexClassification(LasPointClassification classification)
        {
            switch (classification)
            {
                case LasPointClassification.Ground:
                    return VertexClassification.Ground;
                case LasPointClassification.LowVegetation:
                case LasPointClassification.MediumVegetation:
                case LasPointClassification.HighVegetation:
                    return VertexClassification.Tree;
                case LasPointClassification.Building:
                    return VertexClassification.Roof;
                case LasPointClassification.Water:
                    return VertexClassification.Ground;
                case LasPointClassification.CustomDefined_RoofAndWall:
                    return VertexClassification.Roof | VertexClassification.Wall;
                case LasPointClassification.CustomDefined_Wall:
                    return VertexClassification.Wall;
                default:
                    return VertexClassification.Discarded;
            }
        }

        private static LasPointClassification GetPointClassification(VertexClassification classification)
        {
            switch (classification)
            {
                case VertexClassification.Ground:
                    return LasPointClassification.Ground;
                case VertexClassification.Tree:
                    return LasPointClassification.MediumVegetation;
                case VertexClassification.Roof:
                    return LasPointClassification.Building;
                case VertexClassification.Wall:
                    return LasPointClassification.CustomDefined_Wall;
                case VertexClassification.Roof | VertexClassification.Wall:
                    return LasPointClassification.CustomDefined_RoofAndWall;
                default:
                    return LasPointClassification.Unclassified;
            }
        }

        #region LAS Specification Version 1.3 - R11 OCTOBER 24, 2010

        /// <summary>
        /// LAS Specification Version 1.3 - R11 OCTOBER 24, 2010
        /// http://www.asprs.org/a/society/committees/standards/LAS_1_3_r11.pdf
        /// </summary>
        [StructLayout(LayoutKind.Sequential, Pack = 1, CharSet = CharSet.Ansi)]
        private struct LasHeader_v1_3
        {
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
            public char[] FileSignature;

            public ushort FileSourceID;

            public ushort GlobalEncoding;

            public Guid ProjectID;

            public byte VersionMajor;

            public byte VersionMinor;

            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 32)]
            public string SystemIdentifier;

            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 32)]
            public string GeneratingSoftware;

            public ushort FileCreationDayOfYear;

            public ushort FileCreationYear;

            public ushort HeaderSize;

            public uint OffsetToPointData;

            public uint NumberOfVariableLengthRecords;

            public byte PointDataFormatID;

            public ushort PointDataRecordLength;

            public uint NumberOfPointRecords;

            [MarshalAs(UnmanagedType.ByValArray, ArraySubType = UnmanagedType.U4, SizeConst = 5)]
            public uint[] NumberOfPointsByReturn;

            public double ScaleFactorX;

            public double ScaleFactorY;

            public double ScaleFactorZ;

            public double OffsetX;

            public double OffsetY;

            public double OffsetZ;

            public double MaxX;

            public double MinX;

            public double MaxY;

            public double MinY;

            public double MaxZ;

            public double MinZ;

            public ulong StartOfWaveformDataPacketRecord;
        }

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        private struct LasPointDataRecordFormat0_v1_3
        {
            public int X;

            public int Y;

            public int Z;

            public ushort Intensity;

            public byte ReturnNumber_NumberOfReturns_ScanDirectionFlag_EdgeOfFlightLine;

            public byte Classification;

            public byte ScanAngleRank;

            public byte UserData;

            public ushort PointSourceID;
        }

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        private struct LasPointDataRecordFormat1_v1_3
        {
            public int X;

            public int Y;

            public int Z;

            public ushort Intensity;

            public byte ReturnNumber_NumberOfReturns_ScanDirectionFlag_EdgeOfFlightLine;

            public byte Classification;

            public byte ScanAngleRank;

            public byte UserData;

            public ushort PointSourceID;

            public double GPSTime;
        }

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        private struct LasPointDataRecordFormat2_v1_3
        {
            public int X;

            public int Y;

            public int Z;

            public ushort Intensity;

            public byte ReturnNumber_NumberOfReturns_ScanDirectionFlag_EdgeOfFlightLine;

            public byte Classification;

            public byte ScanAngleRank;

            public byte UserData;

            public ushort PointSourceID;

            public ushort Red;

            public ushort Green;

            public ushort Blue;
        }

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        private struct LasPointDataRecordFormat3_v1_3
        {
            public int X;

            public int Y;

            public int Z;

            public ushort Intensity;

            public byte ReturnNumber_NumberOfReturns_ScanDirectionFlag_EdgeOfFlightLine;

            public byte Classification;

            public byte ScanAngleRank;

            public byte UserData;

            public ushort PointSourceID;

            public double GPSTime;

            public ushort Red;

            public ushort Green;

            public ushort Blue;
        }

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        private struct LasPointDataRecordFormat4_v1_3
        {
            public int X;

            public int Y;

            public int Z;

            public ushort Intensity;

            public byte ReturnNumber_NumberOfReturns_ScanDirectionFlag_EdgeOfFlightLine;

            public byte Classification;

            public byte ScanAngleRank;

            public byte UserData;

            public ushort PointSourceID;

            public double GPSTime;

            public byte WavePacketDescriptorIndex;

            public ulong ByteOffsetToWaveformData;

            public uint WaveformPacketSizeInBytes;

            public float ReturnPointWaveformLocation;

            public float Xt;

            public float Yt;

            public float Zt;
        }

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        private struct LasPointDataRecordFormat5_v1_3
        {
            public int X;

            public int Y;

            public int Z;

            public ushort Intensity;

            public byte ReturnNumber_NumberOfReturns_ScanDirectionFlag_EdgeOfFlightLine;

            public byte Classification;

            public byte ScanAngleRank;

            public byte UserData;

            public ushort PointSourceID;

            public double GPSTime;

            public ushort Red;

            public ushort Green;

            public ushort Blue;

            public byte WavePacketDescriptorIndex;

            public ulong ByteOffsetToWaveformData;

            public uint WaveformPacketSizeInBytes;

            public float ReturnPointWaveformLocation;

            public float Xt;

            public float Yt;

            public float Zt;
        }

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        private struct LasPointDataRecordFormat6_Custom
        {
            public Vector3 Position;

            public LasPointClassification Classification;

            public Color4 Color;

            public Vector3 Normal;

            public Vector3 WallNormal;

            public float GroundLevel;
        }

        // LAS Specification Version 1.3 - R11 OCTOBER 24, 2010
        // Table 4.9 - ASPRS Standard LIDAR Point Classes
        public enum LasPointClassification
        {
            Created_NeverClassified = 0,
            Unclassified = 1,
            Ground = 2,
            LowVegetation = 3,
            MediumVegetation = 4,
            HighVegetation = 5,
            Building = 6,
            LowPoint_Noise = 7,
            ModelKeyPoint = 8,
            Water = 9,
            // 10: reserved for ASPRS Definition
            // 11: reserved for ASPRS Definition
            OverlapPoints = 12,
            // 13-31: reserved for ASPRS Definition
            CustomDefined_RoofAndWall = 32,
            CustomDefined_Wall = 33,
        }

        #endregion

        #region LAS Specification Version 2.0 PROPOSAL May 1, 2007

        /// <summary>
        /// LAS Specification Version 2.0 PROPOSAL May 1, 2007
        /// http://www.asprs.org/a/society/committees/standards/asprs_las_format_v20.pdf
        /// </summary>
        [StructLayout(LayoutKind.Sequential, Pack = 1, CharSet = CharSet.Ansi)]
        private struct LasHeader_v2_0
        {
            /// <summary>
            /// The file signature must contain the four characters “LASF”
            /// </summary>
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
            public char[] FileSignature;

            public uint FileID;

            public Guid ProjectID;

            public byte VersionMajor;

            public byte VersionMinor;

            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 32)]
            public string SystemIdentifier;

            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 32)]
            public string GeneratingSoftware;

            public uint FileCreationDate;

            public uint FileCreationTime;

            /// <summary>
            /// This field has a value of 322 for Version 2.0.
            /// </summary>
            public ushort FileHeaderSize;

            public ushort SourceIDPacket;

            public ushort OffsetToPointRecordMetadataBlock;

            public ushort SizeOfPointRecordMetadataBlock;

            public ushort PointDataRecordLength;

            public ulong OffsetToPointData;

            public ulong NumberOfPointRecords;

            public uint OffsetToFirstVariableLengthRecord;

            public uint NumberOfVariableLengthRecords;

            [MarshalAs(UnmanagedType.ByValArray, ArraySubType = UnmanagedType.U4, SizeConst = 16)]
            public uint[] NumberOfPointsByReturn;

            /// <summary>
            /// 0 Point records are compatible with LAS 1.1 point record type 0
            /// 1 Point records are compatible with LAS 1.1 point record type 1
            /// 2-255 Reserved
            /// </summary>
            public byte PointRecordCompatibility;

            /// <summary>
            /// 1 Geographic or projected coordinate system as defined by Well Known Text (WKT) definitions in variable length records
            /// 2 Cartesian, in which case X, Y and Z have their standard Cartesian meaning
            /// 3 Positive Spherical (defined in the positive spherical definition table)
            /// 4 Negative Spherical (defined in the negative spherical definition table)
            /// </summary>
            public byte CoordinateSystemType;

            /// <summary>
            /// 0 Units are defined in the WKT file
            /// 1 Meters
            /// 2 International Feet
            /// 3 U. S. Survey Feet
            /// 4 Arc Seconds (Cartesian coordinate systems only)
            /// </summary>
            public byte MetricUnits;

            /// <summary>
            /// 0 Use units defined in the Metric Units field
            /// 1 Meters
            /// 2 International Feet
            /// 3 U. S. Survey Feet
            /// </summary>
            public byte VerticalMetricUnits;

            public double OriginX;

            public double OriginY;

            public double OriginZ;

            [MarshalAs(UnmanagedType.I1)]
            public bool ApplyScaling;

            public double ScaleFactorX;

            public double ScaleFactorY;

            public double ScaleFactorZ;

            [MarshalAs(UnmanagedType.I1)]
            public bool ApplyOffsets;

            public double OffsetX;

            public double OffsetY;

            public double OffsetZ;

            public double MaxX;

            public double MinX;

            public double MaxY;

            public double MinY;

            public double MaxZ;

            public double MinZ;
        }

        #endregion

        public static void ReadAdditionalClassificationFile(string filename, List<Vertex> vertexList)
        {
            if (Path.GetExtension(filename).ToLower() != "las")
            {
                return;
            }

            filename = Path.ChangeExtension(filename, ".bin");
            if (!File.Exists(filename))
            {
                return;
            }

            using (FileStream fs = new FileStream(filename, FileMode.Open, FileAccess.Read, FileShare.Read))
            {
                for (int i = 0; i < vertexList.Count; i++)
                {
                    var vertex = vertexList[i];
                    vertex.Classification = GetVertexClassification((LasPointClassification)fs.ReadByte());
                }
            }
        }

        private static ReaderWriterLockSlim lazLock = new ReaderWriterLockSlim();

        public static void InitSavingFile(string filename)
        {
            lazFilename = Path.ChangeExtension(filename, ".laz");

            LasHeader_v1_3 header;
            using (FileStream fs = new FileStream(filename, FileMode.Open, FileAccess.Read, FileShare.Read))
            {
                using (BinaryReader br = new BinaryReader(fs))
                {
                    header = ReadStructure<LasHeader_v1_3>(br);
                }
            }

            header.ScaleFactorX = 1;
            header.ScaleFactorY = 1;
            header.ScaleFactorZ = 1;
            header.OffsetToPointData = (uint)Marshal.SizeOf(header);
            header.PointDataFormatID = 6;
            header.PointDataRecordLength = (ushort)Marshal.SizeOf(typeof(LasPointDataRecordFormat6_Custom));
            header.NumberOfPointRecords = 0;

            lazLock.EnterWriteLock();
            {
                using (FileStream fs = new FileStream(lazFilename, FileMode.Create, FileAccess.ReadWrite))
                {
                    using (BinaryWriter bw = new BinaryWriter(fs))
                    {
                        WriteStructure(bw, header);
                    }
                }
            }
            lazLock.ExitWriteLock();
        }

        public static void SaveVertices(List<Vertex>[] vertexLists, out int startIndex, out int count)
        {
            count = 0;
            for (int i = 0; i < vertexLists.Length; i++)
            {
                count += vertexLists[i].Count;
            }


            lazLock.EnterWriteLock();
            {
                LasHeader_v1_3 header;
                using (FileStream fs = new FileStream(lazFilename, FileMode.Open, FileAccess.ReadWrite))
                {
                    using (BinaryReader br = new BinaryReader(fs))
                    {
                        header = ReadStructure<LasHeader_v1_3>(br);

                        startIndex = (int)header.NumberOfPointRecords;
                        header.NumberOfPointRecords = (uint)(startIndex + count);

                        using (BinaryWriter bw = new BinaryWriter(fs))
                        {
                            bw.Seek(0, SeekOrigin.Begin);
                            WriteStructure(bw, header);

                            bw.Seek(0, SeekOrigin.End);
                            for (int i = 0; i < vertexLists.Length; i++)
                            {
                                foreach (var vertex in vertexLists[i])
                                {
                                    LasPointDataRecordFormat6_Custom point6 = new LasPointDataRecordFormat6_Custom();
                                    point6.Position = vertex.Position;
                                    point6.Classification = GetPointClassification(vertex.Classification);
                                    point6.Color = vertex.Color;
                                    point6.Normal = vertex.Normal;
                                    point6.WallNormal = vertex.WallNormal;
                                    point6.GroundLevel = vertex.GroundLevel;
                                    WriteStructure(bw, point6);
                                }
                            }
                        }
                    }
                }
            }
            lazLock.ExitWriteLock();
        }

        public static void FinishSavingFile(Vector3 min, Vector3 max, float vertexAvgSize)
        {
            lazLock.EnterWriteLock();
            {
                LasHeader_v1_3 header;
                using (FileStream fs = new FileStream(lazFilename, FileMode.Open, FileAccess.ReadWrite))
                {
                    using (BinaryReader br = new BinaryReader(fs))
                    {
                        header = ReadStructure<LasHeader_v1_3>(br);

                        header.MinX = min.X;
                        header.MinY = min.Y;
                        header.MinZ = min.Z;
                        header.MaxX = max.X;
                        header.MaxY = max.Y;
                        header.MaxZ = max.Z;
                        header.OffsetX = min.X;
                        header.OffsetY = min.Y;
                        header.OffsetZ = min.Z;
                        header.ScaleFactorX = vertexAvgSize;

                        using (BinaryWriter bw = new BinaryWriter(fs))
                        {
                            bw.Seek(0, SeekOrigin.Begin);
                            WriteStructure(bw, header);
                        }
                    }
                }
            }
            lazLock.ExitWriteLock();
        }

        public static void SaveColor(QuadTreeNode node, Color4[] colorList)
        {
            int headerSize = Marshal.SizeOf(typeof(LasHeader_v1_3));
            int colorFieldOffset = Marshal.OffsetOf(typeof(LasPointDataRecordFormat6_Custom), "Color").ToInt32();
            int format6Size = Marshal.SizeOf(typeof(LasPointDataRecordFormat6_Custom));
            int offsetGap = format6Size - Marshal.SizeOf(typeof(Color4));

            lazLock.EnterWriteLock();
            {
                using (FileStream fs = new FileStream(lazFilename, FileMode.Open, FileAccess.ReadWrite))
                {
                    using (BinaryWriter bw = new BinaryWriter(fs))
                    {
                        int index = 0;
                        foreach (var item in node.VertexIndexLink)
                        {
                            fs.Seek(headerSize + (long)item.StartIndex * format6Size + colorFieldOffset, SeekOrigin.Begin);
                            for (int i = item.StartIndex; i <= item.EndIndex; i++)
                            {
                                WriteStructure(bw, colorList[index]);
                                fs.Seek(offsetGap, SeekOrigin.Current);
                                index++;
                            }
                        }

                        Debug.Assert(index == colorList.Length);
                    }
                }
            }
            lazLock.ExitWriteLock();
        }

        public static void FinishSavingColor()
        {
            lazLock.EnterWriteLock();
            {
                LasHeader_v1_3 header;
                using (FileStream fs = new FileStream(lazFilename, FileMode.Open, FileAccess.ReadWrite))
                {
                    using (BinaryReader br = new BinaryReader(fs))
                    {
                        header = ReadStructure<LasHeader_v1_3>(br);
                        header.PointDataFormatID = 7;

                        using (BinaryWriter bw = new BinaryWriter(fs))
                        {
                            bw.Seek(0, SeekOrigin.Begin);
                            WriteStructure(bw, header);
                        }
                    }
                }
            }
            lazLock.ExitWriteLock();
        }

    }
}
